#include "RGBtoYUV.h"

RGBtoYUV::RGBtoYUV()
{
}

RGBtoYUV::~RGBtoYUV()
{
}
	

// Conver an RGB image into YUV 4-4-4
cv::Mat RGBtoYUV::ConvertRgbToYuv(cv::Mat &img)
{
	cv::Mat yuv411(img.size().height, img.size().width, img.type());
   
	for (int i = 0; i < img.size().height; ++i)
	  {
		  for (int j = 0; j < img.size().width; ++j)
		  {
			  cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
			 int B = pixel.val[0];
			 int G = pixel.val[1];
			 int R = pixel.val[2];

			int Y = ((66 * R + 129 * G + 25 * B + 128) >> 8) + 16;
			int U = ((-38 * R - 74 * G + 112 * B + 128) >> 8)+ 128;
			int V = ((112 * R - 94 * G - 18 * B + 128) >> 8) + 128;

			pixel.val[0] = U;//U;
			pixel.val[1] = Y;//Y;
			pixel.val[2] = V;//V;

			yuv411.at<cv::Vec3b>(i, j) = pixel;
		  }
	  }
	return yuv411;
}

// Conver an YUV image 4-4-4 into YUV 4-1-1
cv::Mat RGBtoYUV::ConvertYUV_444_To_411(cv::Mat &img)
{
	cv::Mat yuv411(img.size().height, img.size().width, img.type());
	 for (int i = 0; i < img.size().height / 2; ++i)
	  {
		  for (int j = 0; j < img.size().width / 2; ++j)
		  {
			   cv::Vec3b pixel = img.at<cv::Vec3b>(2 * i, 2 * j);

			   cv::Vec3b pixelYuv = yuv411.at<cv::Vec3b>(i, j);
			  
			   pixelYuv.val[0] = pixel.val[0];
			   pixelYuv.val[2] = pixel.val[2];

			   yuv411.at<cv::Vec3b>(i, j) = pixelYuv;
		  }
	 }
	for (int i = 0; i < img.size().height; ++i)
	  {
		  for (int j = 0; j < img.size().width; ++j)
		  {
			  cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
			  cv::Vec3b pixelYuv = yuv411.at<cv::Vec3b>(i, j);

			  pixelYuv.val[1] = pixel.val[1];
			  yuv411.at<cv::Vec3b>(i, j) = pixelYuv;
		  }
	}
	return yuv411;
}

cv::Vec3b Pixel411ToRgb(cv::Vec3b pixelYUV, int lumi)
{
	cv::Vec3b pixelRGB;

	int U = pixelYUV.val[0];
	int Y = lumi;//pixelYUV.val[1];
	int V = pixelYUV.val[2];
	
	int C = Y - 16;
	int D = U - 128;
	int E = V - 128;

	int R = CLIP(( 298 * C + 409 * E + 128) >> 8);
	int G = CLIP(( 298 * C - 100 * D - 208 * E + 128) >> 8);
	int B = CLIP(( 298 * C + 516 * D + 128) >> 8);

	pixelRGB.val[0] = B;
	pixelRGB.val[1] = G;
	pixelRGB.val[2] = R;
	
	return pixelRGB;
}

//Convert an YUV 4-1-1 image into RGB
cv::Mat  RGBtoYUV::ConvertYuv411ToRgb(cv::Mat &img, int width, int height)
{	
	cv::Mat rgb(img.size().height, img.size().width, img.type());

	for (int i = 0; i < img.size().height; ++i)
	  {
		  for (int j = 0; j < img.size().width; ++j)
		  {
			  cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
			  cv::Vec3b pixelYuv = rgb.at<cv::Vec3b>(i, j);

			  pixelYuv.val[1] = pixel.val[1];
			  rgb.at<cv::Vec3b>(i, j) = pixelYuv;
		  }
	}

	 for (int i = 0; i < img.size().height / 2; ++i)
	  {
		  for (int j = 0; j < img.size().width /2; ++j)
		  {
			    cv::Vec3b yuv1 = img.at<cv::Vec3b>(i, j);

				rgb.at<cv::Vec3b>(i*2, j*2) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2, j*2).val[1]);
			   rgb.at<cv::Vec3b>(i*2, j*2 + 1) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2, j*2 + 1).val[1]);;
			   rgb.at<cv::Vec3b>(i*2 + 1, j*2) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2 + 1, j*2).val[1]);;
			 rgb.at<cv::Vec3b>(i*2 + 1, j *2+ 1) = Pixel411ToRgb(yuv1, img.at<cv::Vec3b>(i*2 + 1, j *2+ 1).val[1]);;
		  }
	 }

	return rgb;
}


//Convert an YUV 4-4-4 image into RGB
void  RGBtoYUV::ConvertYuvToRgb(cv::Mat &img, int width, int height) const
{	
	for (int i = 0; i < height; ++i)
	  {
		  for (int j = 0; j < width; ++j)
		  {
			 cv::Vec3b pixel = img.at<cv::Vec3b>(i, j);
			 
			 int U = pixel.val[0];
			 int Y = pixel.val[1];
			 int V = pixel.val[2];
		
			int C = Y - 16;
			int D = U - 128;
			int E = V - 128;

			int R = CLIP(( 298 * C + 409 * E + 128) >> 8);
			int G = CLIP(( 298 * C - 100 * D - 208 * E + 128) >> 8);
			int B = CLIP(( 298 * C + 516 * D + 128) >> 8);

			 pixel.val[0] = B;
			 pixel.val[1] = G;
			 pixel.val[2] = R;

			 img.at<cv::Vec3b>(i, j) = pixel;
		  }
	}
}

